#pragma once

#include "vec2.h"
#include <math.h>

OPEN_JLIB_NS


struct Vec3 {

  union {
    struct { f32 x, y, z; };
    struct { f32 r, g, b; };
    f32v4 p;
  };


  inline Vec3 () noexcept = default;
  inline Vec3 (f32v4 p) noexcept : p(v) {
  }
  inline Vec3 (f32 x, f32 y, f32 z = 0.0F) noexcept {
    asm volatile (R"(
    zip1 %[dst].2s, %[op1].2s, %[op2].2s
    INS %[dst].s[2], %[op3].s[0])"
    : [dst]"=&x"(p): [op1]"x"(x), [op2]"x"(y), [op3]"x"(z));
  }
  inline Vec3 (Vec2 v, f32 z = 0.0F) noexcept {
    asm volatile (R"(
    ins %[dst].s[2], %[op2].s[0])"
    : [dst]"=x"(p): "[dst]"(v.p), [op2]"x"(z));
  }
  inline Vec3 (f32 s) noexcept {
    asm volatile (R"(
    dup %[dst].4s, %[src].s[0])"
    : [dst]"=x"(p): [src]"x"(s));
  }
  inline Vec3 (const f32* m) noexcept {
    asm volatile (R"(
    ld1 { %[dst].2s }, [%[mem]], #8
    ld1 { %[dst].s }[2], [%[mem]], #4)"
    : [dst]"=x"(p), [mem]"+r"(m));
  }


  inline f32 operator[] (uwl i) const noexcept {
    return p[i];
  }


  inline f32 x () const noexcept {
    return p[0];
  }
  inline f32 y () const noexcept {
    return p[1];
  }
  inline f32 z () const noexcept {
    return p[2];
  }


  inline Vec3& x (f32 s) noexcept {
    p[0] = s;
    return *this;
  }
  inline Vec3& y (f32 s) noexcept {
    p[1] = s;
    return *this;
  }
  inline Vec3& z (f32 s) noexcept {
    p[2] = s;
    return *this;
  }


  inline Vec3& operator+= (Vec3 v) noexcept {
    p += v.p;
    return *this;
  }
  inline Vec3& operator-= (Vec3 v) noexcept {
    p -= v.p;
    return *this;
  }
  inline Vec3& operator*= (Vec3 v) noexcept {
    p *= v.p;
    return *this;
  }
  inline Vec3& operator/= (Vec3 v) noexcept {
    p /= v.p;
    return *this;
  }


  inline Vec3& operator*= (f32 s) noexcept {
    p *= s;
    return *this;
  }
  inline Vec3& operator/= (f32 s) noexcept {
    p /= s;
    return *this;
  }


  inline Vec3& clamp (Vec3 min, Vec3 max) noexcept {
    asm volatile (R"(
    fmin %[dst].4s, %[dst].4s, %[max].4s
    fmax %[dst].4s, %[dst].4s, %[min].4s)"
    : [dst]"+x"(p): [min]"x"(min.p), [max]"x"(max.p));
    return *this;
  }
  inline f32 normalize () noexcept {
    f32 r;
    asm volatile (R"(
    eor %[dst].16b, %[dst].16b, %[dst].16b
    ins %[src].s[3], %[dst].s[0]
    fmul %[dst].4s, %[src].4s, %[src].4s
    faddp %[dst].4s, %[dst].4s, %[dst].4s
    faddp %[dst].4s, %[dst].4s, %[dst].4s
    fsqrt %[dst].4s, %[dst].4s
    fdiv %[src].4s, %[src].4s, %[dst].4s)"
    : [dst]"=r"(r), [src]"+x"(p));
    return r;
  }


  inline Vec3 operator+ (Vec3 v) const noexcept {
    return p + v.p;
  }
  inline Vec3 operator- (Vec3 v) const noexcept {
    return p - v.p;
  }
  inline Vec3 operator* (Vec3 v) const noexcept {
    return p * v.p;
  }
  inline Vec3 operator/ (Vec3 v) const noexcept {
    return p / v.p;
  }


  inline Vec3 operator* (f32 s) const noexcept {
    return p * s;
  }
  inline Vec3 operator/ (f32 s) const noexcept {
    return p / s;
  }


  inline f32 operator& (Vec3 v) const noexcept {
    f32 r;
    asm volatile (R"(
    fmul %[op2].4s, %[op1].4s, %[dst].4s
    faddp %s[dst], %[dst].2s
    ins %[dst].s[1], %[dst].s[2]
    faddp %s[dst], %[dst].2s)"
    : [dst]"=x"(r): [op1]"x"(p), [op2]"x"(v.p));
    return r;
  }
  inline Vec3 operator^ (Vec3 v) const noexcept {
    static const u8v16 i = {4,5,6,7,8,9,10,11,0,1,2,3,12,13,14,15};
    f32v4 r, t0, t1;
    asm volatile (R"(
    tbl %[tv0]16b, { %[op1].16b }, %[idx]
    tbl %[tv1]16b, { %[op2].16b }, %[idx]
    tbl %[dst]16b, { %[tv1].16b }, %[idx]
    fmul %[dst].4s, %[tv0].4s, %[dst].4s
    tbl %[tv0].16b, { %[tv0].16b }, %[idx]
    fmls %[dst].4s, %[tv0].4s, %[tv1].4s)"
    : [dst]"=&x"(r), [tv0]"=&x"(t0), [tv1]"=&x"(t1): [op1]"x"(p), [op2]"x"(v.p), [idx]"x"(i));
    return r;
  }


  inline Vec3 operator+ () const noexcept {
    f32v4 r;
    asm volatile (R"(
    fabs %[dst].4s, %[src].4s)"
    : [dst]"=x"(r): [src]"x"(p));
    return r;
  }
  inline Vec3 operator- () const noexcept {
    f32v4 r;
    asm volatile (R"(
    fneg %[dst].4s, %[src].4s)"
    : [dst]"=x"(r): [src]"x"(p));
    return r;
  }
  inline Vec3 operator* () const noexcept {
    f32v4 r;
    asm volatile (R"(
    fmul %[dst].4s, %[src].4s, %[src].4s
    faddp %s[dst], %[dst].2s
    ins %[dst].s[1], %[dst].s[2]
    faddp %s[dst], %[dst].2s
    fsqrt %s[dst], %s[dst]
    dup %[dst].4s, %[dst].s[0]
    fdiv %[dst].4s, %[src].4s, %[dst].4s)"
    : [dst]"=&x"(r): [src]"x"(p));
    return r;
  }
  inline f32 operator~ () const noexcept {
    f32 r;
    asm volatile (R"(
    fmul %[dst].4s, %[src].4s, %[src].4s
    faddp %s[dst], %[dst].2s
    ins %[dst].s[1], %[dst].s[2]
    faddp %s[dst], %[dst].2s
    fsqrt %s[dst], %s[dst])"
    : [dst]"=x"(r): [src]"x"(p));
    return r;
  }


  inline Vec3 luminance () const noexcept {
    static const f32v4 f = {0.299F, 0.587F, 0.114F, 0.0F};
    f32v4 r;
    asm volatile (R"(
    fmul %[dst].4s, %[op1].4s, %[op2].4s
    faddp %s[dst], %[dst].2s
    ins %[dst].s[1], %[dst].s[2]
    faddp %s[dst], %[dst].2s
    fsqrt %s[dst], %s[dst]
    dup %[dst].4s, %[dst].s[0])"
    : [dst]"=x"(r): [op1]"x"(p), [op2]"x"(f));
    return r;
  }


  inline static Vec3 zero () noexcept {
    return {0.0F, 0.0F, 0.0F};
  }
  inline static Vec3 one () noexcept {
    f32v4 r;
    asm volatile (R"(
    dup %[dst].4s %[one].s[0])"
    : [dst]"=x"(r): [one]"x"(1.0F));
    return r;
  }
  template<u8 Com>
  inline static Vec3 unit () noexcept;


  inline static f32 distance (Vec3 a, Vec3 b) noexcept {
    return ~(b-a);
  }
  inline static Vec3 min (Vec3 a, Vec3 b) noexcept {
    f32v4 r;
    asm volatile (R"(
    fmax %[dst].4s, %[op1].4s, %[op2].4s)"
    : [dst]"=x"(r): [op1]"x"(a.p), [op2]"x"(b.p));
    return r;
  }
  inline static Vec3 max (Vec3 a, Vec3 b) noexcept {
    f32v4 r;
    asm volatile (R"(
    fmin %[dst].4s, %[op1].4s, %[op2].4s)"
    : [dst]"=x"(r): [op1]"x"(a.p), [op2]"x"(b.p));
    return r;
  }
  inline static Vec3 clamp (Vec3 v, Vec3 min, Vec3 max) noexcept {
    return Vec3::min(Vec3::max(v, min), max);
  }
  inline static Vec3 lerp (Vec3 a, Vec3 b, Vec3 t) noexcept {
    return a + (b-a)*t;
  }
  inline static Vec3 slerp (Vec3 a, Vec3 b, f32 t) noexcept {
    auto La = a.normalize();
    auto Lb = b.normalize();
    auto Lt = La + (Lb-La)*t;
    auto Ln = ~(a^b);
    auto R = asinf(Ln);
    auto r = R * t;
    auto p = sinf(R-r) / Ln;
    auto q = sinf(r) / Ln;
    return (a*p + b*q) * Lt;
  }
};


template<>
inline Vec3 Vec3::unit<0> () noexcept {
  f32v4 r;
  asm volatile (R"(
  eor %[dst].16b, %[dst].16b, %[dst].16b
  ins %[dst].s[0], %[one].s[0])"
  : [dst]"=&x"(r): [one]"x"(1.0F));
  return r;
}
template<>
inline Vec3 Vec3::unit<1> () noexcept {
  f32v4 r;
  asm volatile (R"(
  eor %[dst].16b, %[dst].16b, %[dst].16b
  ins %[dst].s[1], %[one].s[0])"
  : [dst]"=&x"(r): [one]"x"(1.0F));
  return r;
}
template<>
inline Vec3 Vec3::unit<2> () noexcept {
  f32v4 r;
  asm volatile (R"(
  eor %[dst].16b, %[dst].16b, %[dst].16b
  ins %[dst].s[2], %[one].s[0])"
  : [dst]"=&x"(r): [one]"x"(1.0F));
  return r;
}


inline Vec3 operator* (f32 a, Vec3 b) noexcept {
  return b * a;
}


inline Vec3 operator"" _RGB (unsigned long long c) noexcept {
  auto i = (u32)c;
  auto s = 1.0F / 255.0F;
  auto r = ((i >> 16) & 0xFF) * s;
  auto g = ((i >> 8) & 0xFF) * s;
  auto b = (i & 0xFF) * s;
  return {r, g, b};
}

inline Vec3 operator"" _LUM (unsigned long long c) noexcept {
  auto v = ((u32)c & 0xFF) * (1.0F / 255.0F);
  return {v, v, v};
}

inline Vec3 operator"" _YUV (unsigned long long c) noexcept {
  auto i = (u32)c;
  auto s = 1.0F / 255.0F;
  auto y = ((i >> 16) & 0xFF) * s;
  auto u = ((i >> 8) & 0xFF) * s;
  auto v = (i & 0xFF) * s;
  auto r = y + 1.14F * v;
  auto g = y - 0.39F * u - 0.58F * v;
  auto b = y + 2.03F * u;
  return {r, g, b};
}

inline Vec3 operator"" _HSV (unsigned long long c) noexcept {
  auto v = (c % 100) * 0.01F;
  auto s = (c / 100 % 100) * 0.01F;
  auto h = (c / 10000 % 360) * (1.0F / 60.0F);
  auto i = (s32)h;
  auto f = h - i;
  auto x = v * (1.0F - s);
  auto y = v * (1.0F - s * f);
  auto z = v * (1.0F - s * (1.0F - f));
  auto r = 0.0F;
  auto g = 0.0F;
  auto b = 0.0F;
  switch (i) {
    case 0: r = v; g = z; b = x; break;
    case 1: r = y; g = v; b = x; break;
    case 2: r = x; g = v; b = z; break;
    case 3: r = x; g = y; b = v; break;
    case 4: r = z; g = x; b = v; break;
    case 5: r = v; g = x; b = y; break;
    default: break;
  }
  return {r, g, b};
}


CLOSE_JLIB_NS
